function ctrl = GuidanceGli(t, tf, statec, statef, ctrlp)
global m d2r R0 g0 S alphaMax alphaLD alphaMin Stogo0 delta_azi0 delta_azif absBankMax kh2
rad  = statec(1);
lon = statec(2);
lat = statec(3);
V = statec(4);
fpa = statec(5);
azi = statec(6);
radf  = statef(1);
lonf = statef(2);
latf = statef(3);
Vf = statef(4);
fpaf = statef(5);
azif = statef(6);
alphap = ctrlp(1);
signOfBankp = ctrlp(3);
%% AOA
[~,sonic,~,rho] = atmoscoesa(rad - R0);
Ma = V/sonic;
[CL, ~] = getCLCD(alphaLD, Ma);
[~, CD] = getCLCD(alphap, Ma);
q = 0.5*rho*V^2;
L = q*S*CL;
D = q*S*CD;
dhc = - (D/m) * 2 * 7110 / V;
dh = V * sin(fpa);
Ld = L/m + kh2 * (dhc - dh);
fun = @(alpha)abs(Ld - ...
      q*S/m*(-0.21 + 0.005059*Ma + 0.0497*alpha/d2r + 0.0004244*(alpha/d2r).^2));
alphad = fminbnd(fun, alphaMin, alphaMax);
% alphad = alphaLD;
%% magnitude of bank
LtoD = 3;
% t0_go = LtoD*sqrt(R0/g)/2*log((sqrt(g*R0)+V)*(sqrt(g*R0)-Vf)...
%         /(sqrt(g*R0)-V)*(sqrt(g*R0)+Vf));
% tast_go = tf - t;
% abs_bank = acos(tast_go/t0_go); 
Stogo = acos(sin(lat)*sin(latf) + cos(lat)*cos(latf)*cos(lonf-lon));
StogoEstFun = @(abs_bank)abs(Stogo - LtoD/2*cos(abs_bank)*log((g0*R0-Vf*Vf)/(g0*R0-V*V)));
absBank = fminbnd(StogoEstFun, 0, absBankMax);
%% sign of bank
% 藏路尧 基于横程角的侧向走廊
aziLos = asin(sin(lon-lonf)*cos(latf)/sin(Stogo));
Z = asin(sin(Stogo)*sin(azi+aziLos)); % 横程
delta_aziref = (delta_azi0 - delta_azif)*Stogo/Stogo0 + delta_azif;
Zup = asin(sin(Stogo)*sin(delta_aziref));
Zdown = -Zup;
% Constrained Predictor–Corrector Entry Guidance
% aziLos = asin(sin(lon-lonf)*cos(latf)/sin(Stogo));
% Z = R0*asin(sin(Stogo)*sin(azi - aziLos));
% Zup = 1500 + 200*(V - Vf);
% Zdown = -Zup;
% 视线角走廊
% aziLos = atan(sin(lonf-lon)/(cos(lat)*tan(latf) - sin(lat)*cos(lonf-lon)));
% Z = azi - aziLos;
% Zup = 10*pi/180;
% Zdown = -10*pi/180;

if Z > Zup
    signOfBank = -1;
elseif Z > Zdown
    signOfBank = signOfBankp;
else
    signOfBank = 1;
end

ctrl(1) = alphad;
ctrl(2) = absBank;
ctrl(3) = signOfBank;
end